
import sensor, image, time
from machine import UART
from machine import LED

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.set_auto_exposure(False) # 关闭自动曝光
sensor.set_auto_gain(False)     # 关闭自动增益
#sensor.set_vflip (True)
#sensor.set_hmirror (True)
sensor.skip_frames(time = 1000)

uart = UART(3, 9600)              # UART(3)P4-TX P5-RX
uart.init(9600,bits=8,parity=None,stop=1)
clock = time.clock()
led = LED("LED_BLUE")
led.on()

red_threshold = [(66, 93, 2, 35, -4, 4)]
green_threshold = [(90, 100, -10, 10, -10, 6)]
green_threshold1 = [(97, 100, -3, 6, -6, 5)]

center_x = 0
center_y = 0

# 查找中心方框
def find_center_rect(img):
    rect = 0
    megnitude_max = 0
    for i in img:
        rect_center_x = i.w() /2 + i.x()
        rect_center_y = i.h() /2 + i.y()
        # 计算矩形中心点与图像中心点的距离
        distance_to_center = (abs(center_x - rect_center_x)**2 + abs(center_y - rect_center_y)**2)**0.5
        # 计算矩形左上角点与图像中心点的距离
        distance_to_point = (abs(i.x() - rect_center_x)**2 + abs(i.y() - rect_center_y)**2)**0.5
        if distance_to_center < 40 and i.magnitude() > megnitude_max and distance_to_point < 60:
            megnitude_max = i.magnitude()
            rect = i
    return rect

rect_center_x = 0   # 记录方框的中心点
rect_center_y = 0
rect_dis = 10        # 记录方框的中点到四个点的距离
def find_dots(rect,img):
    global rect_center_x,rect_center_y,rect_dis
    area = 0
    dots = 0
    if rect != 0:
        rect_center_x = rect.w() / 2 + rect.x()
        rect_center_y = rect.h() / 2 + rect.y()
        rect_dis = ((rect.w() / 2)**2 + (rect.h() / 2)**2)**0.5
    for i in img:
        distance_ = (abs(rect_center_x - i.cx())**2 + abs(rect_center_y - i.cy())**2)**0.5
        if distance_ < rect_dis :
            if i.area() > area:
                area = i.area()
                dots = i

    return dots

red_point = [0,0]
dots_rect = 0
def find_dotss(img,rect):
    global dots_rect
    max_light = 0
    if rect != 0:
        dots_rect = rect
    if dots_rect != 0:
        for i in range(dots_rect.x()-20,dots_rect.x()+dots_rect.w()+20):
            for j in range(dots_rect.y()-20,dots_rect.y()+dots_rect.h()+20):
                rgbtemp = img.get_pixel(i,j)
                if isinstance(rgbtemp,tuple):
                    rgb = rgbtemp[0] + rgbtemp[1] + rgbtemp[2]
                    #if i == 67 and j == 62:print(str(rgbtemp[0]) + ':' + str(rgbtemp[1]) + ':' + str(rgbtemp[2]))
                    #if rgbtemp[0] > 240 and rgbtemp[1] > 240 and rgbtemp[2] > 240:
                    if rgb > 240*3:
                        rgbtemp1 = img.get_pixel(i+1,j)
                        rgb1 = rgbtemp1[0]+rgbtemp1[1]+rgbtemp1[2]
                        rgbtemp2 = img.get_pixel(i-1,j)
                        rgb2 = rgbtemp2[0]+rgbtemp2[1]+rgbtemp2[2]
                        rgbtemp3 = img.get_pixel(i,j+1)
                        rgb3 = rgbtemp3[0]+rgbtemp3[1]+rgbtemp3[2]
                        rgbtemp4 = img.get_pixel(i,j-1)
                        rgb4 = rgbtemp4[0]+rgbtemp4[1]+rgbtemp4[2]
                        if rgb1 > 240*3 or rgb2 > 240*3 or rgb3 > 240*3 or rgb4 > 240*3:
                            max_light=rgb
                            red_point[0]=i
                            red_point[1]=j
                            return red_point
    return red_point

coor = [(0,0),(0,0),(0,0),(0,0)]
data = [0] * 13
def Send_coordinate(rect,red):
    global coor,data
    # 查找ABCD点坐标
    l = 0
    if rect != 0:
        for i in rect.corners():
            coor[l] = i
            l += 1
        # 根据Y轴分出上下点
        for i in range(0,len(coor)-1):
            for j in range(i+1,len(coor)):
                if coor[i][1] > coor[j][1]:
                    t = coor[i]
                    coor[i] = coor[j]
                    coor[j] = t
        # 根据X轴分出左右点
        if coor[0][0] > coor[1][0]:
            t = coor[0]
            coor[0] = coor[1]
            coor[1] = t
        if coor[2][0] < coor[3][0]:
            t = coor[2]
            coor[2] = coor[3]
            coor[3] = t
    # 保存圆点中心坐标
    if red != 0:
        data[10] = red[0]
        data[11] = red[1]
#        data[10] = red.cx()
#        data[11] = red.cy()
    # 串口发送数据
    data[0] = 0xa0
    data[1] = 0xb0
    l = 2
    for i in range(0,len(coor)):
        data[l]   = coor[i][0]
        data[l+1] = coor[i][1]
        l += 2
    data[12] = 0xc0
    print(data)
    uart.write(bytearray(data))
    time.sleep(0.01)


while(True):
    clock.tick()
    img = sensor.snapshot()

    center_x = img.width() // 2     # 图像中心点的X坐标
    center_y = img.height() // 2    # 图像中心点的Y坐标
    rect = find_center_rect(img.find_rects(threshold = 50000))  # 查找方框
    #red = find_dots(rect,img.find_blobs(green_threshold))              # 查找圆点
    red = find_dotss(img,rect)
    #red = img.find_blobs(red_threshold)
    # 发送四个点坐标给MCU
    Send_coordinate(rect,red)
    # 画图
    if rect != 0:
        img.draw_rectangle(rect.rect(), color = (255, 0, 0))
        for p in rect.corners():
            img.draw_string (p[0], p[1], str(p[0])+','+str(p[1]), color=(255, 255, 255),mono_space=False)

#    for r in red:
#        img.draw_circle(r.cx(), r.cy(), 5, color = (0, 255, 0))
#        img.draw_string (r.cx(), r.cy(), str(r.area()), color=(255, 255, 255),mono_space=False)
#    if red != 0:
#        img.draw_circle(red.cx(), red.cy(), 5, color = (0, 255, 0))
#        img.draw_string (red.cx(), red.cy(), str(red.cx())+','+str(red.cy()), color=(255, 255, 255),mono_space=False)
    if red[0] != 0 and red[1] != 0:
        img.draw_circle(red[0], red[1], 3, color = (0, 255, 0))
        img.draw_string (red[0], red[1], str(red[0])+','+str(red[1]), color=(255, 255, 255),mono_space=False)



